\documentclass[]{article}

\usepackage{amsmath, amssymb, graphics, setspace}
\usepackage{graphicx}
\newcommand{\mathsym}[1]{{}}
\newcommand{\unicode}[1]{{}}

\newcounter{mathematicapage}

%opening
\title{Optimal Control - Homework Excerise 2}
\author{  Henrik Edlund, 900202-4736, hened061\\ 
Johan Andersson 900612-0332, johan474}

\begin{document}

\maketitle
\subsection*{a)} 
To determine H,G, $\omega$ and S Mathematica was used. The following definitions were made to define the MPC-problem.

\begin{doublespace}
\noindent
\textbf{X=\{\{\text{xk0}\},\{\text{xk1}\}\}\\
U= \text{Transpose}[\{\{\text{uk},\text{uk1}\}\}]\\
\text{TH}=\{\{1\},\{1\}\}\\
\text{GA} = \{\{0,0\},\{1,0\}\}\\
\text{Qm}=\{\{Q ,0\},\{0, Q\}\}\\
\text{Rm}= \{\{R,0\},\{0,R\}\}\\
\text{EPSu}=\{\{1,0\},\{0,1\},\{-1,0\},\{0,-1\}\}\\
\text{Fu} =\text{Transpose}[\{\{1,1,1,1\}\}]}
\end{doublespace}
\noindent
Then following code were used to calculate H,G, $\omega$ and S.

\begin{doublespace}
\noindent
\textbf{H= \text{Transpose}[\text{GA}].\text{Qm}.\text{GA}+\text{Rm}\\
F=\text{Transpose}[\text{GA}].\text{Qm}.\text{TH}\\
z=\text{Sqrt}[2](U+\text{Inverse}[H].F \text{xk0})\\
G = 1/\text{Sqrt}[2]\text{EPSu}\\
S=\text{EPSu}.\text{Inverse}[H].F\\
w = \text{Fu}}
\end{doublespace}
Which resulted in following values on H,G, $\omega$ and S expressed in R and Q.
\begin{equation*}
H = \begin{bmatrix}
Q+R & 0  \\[0.3em]
0 & R            \\[0.3em]
\end{bmatrix}
, \qquad
G =  \begin{bmatrix}
\frac{1}{\sqrt{2}}& 0  \\[0.3em]
0 & \frac{1}{\sqrt{2}} \\[0.3em]
- \frac{1}{\sqrt{2}}& 0  \\[0.3em]
0 & - \frac{1}{\sqrt{2}} \\[0.3em]
\end{bmatrix}
, \qquad
\omega =  \begin{bmatrix}
1    \\[0.3em]
1              \\[0.3em]
1    \\[0.3em]
1              \\[0.3em]
\end{bmatrix}
, \qquad
 S =  \begin{bmatrix}
 \frac{QR}{QR+R^2} & 0  \\[0.3em]
 0 & - \frac{QR}{QR+R^2}            \\[0.3em]
\end{bmatrix}
\end{equation*}




\subsection*{b)}
When $x_k =\theta=0$ the following expression (from Mathematica) of z is
\begin{equation*}
z =\sqrt{2}(U+H^{-1}Fx_k)= \begin{bmatrix}
\sqrt{2}u_k   \\[0.3em]
\sqrt{2}u_{k+1} \\[0.3em]
\end{bmatrix}
\end{equation*}
and the minimizing $u_k$ and $u_{k+1}$ is then calculated from
\begin{equation*}
\min\limits_{z} \frac{1}{2}z^THz =...= \min\limits_{z} \frac{1}{2}\sqrt{2}((Q+R)u_k^2 +Ru_{k+1}^2) 
\end{equation*}
Stationary points can be obtained by calculating the gradient and the Hessian can be used to see if the obtained point is a max/min/saddle point.
\begin{equation*}
 \nabla (\frac{1}{2}\sqrt{2}((Q+R)u_k^2 +Ru_{k+1}^2)) = 
 \begin{bmatrix}
 \sqrt{2}(Q+R)u_k   \\[0.3em]
 \sqrt{2}Ru_{k+1} \\[0.3em]
 \end{bmatrix}
 = \bar{0}
\end{equation*}
Because Q and R are both positive definite  $u_k = 0$ and $u_{k+1} = 0$ to reach the stationary point.
\begin{equation*}
 \nabla^2 (\frac{1}{2}\sqrt{2}((Q+R)u_k^2 +Ru_{k+1}^2))) = 
 \begin{bmatrix}
 \sqrt{2}(Q+R)   & 0\\[0.3em]
 0 &\sqrt{2}R \\[0.3em]
 \end{bmatrix}
\end{equation*}
Which is positive definite which means that the stationary point is a global minimum where
$u_k$ = 0 and
$u_{k+1}$ = 0 \\
This means that \\
\begin{equation*}
z = \begin{bmatrix}
0  \\[0.3em]
0 \\[0.3em]
\end{bmatrix}
\end{equation*}
Checking the boundaries:
\begin{equation*}
\begin{bmatrix}
\frac{1}{\sqrt{2}}& 0  \\[0.3em]
0 & \frac{1}{\sqrt{2}} \\[0.3em]
- \frac{1}{\sqrt{2}}& 0  \\[0.3em]
0 & - \frac{1}{\sqrt{2}} \\[0.3em]
\end{bmatrix}
\begin{bmatrix}
0  \\[0.3em]
0 \\[0.3em]
\end{bmatrix}
<
\begin{bmatrix}
1    \\[0.3em]
1              \\[0.3em]
1    \\[0.3em]
1              \\[0.3em]
\end{bmatrix}
\end{equation*}
All boundaries are fullfilled.
\begin{equation*}
z^* = \begin{bmatrix}
0  \\[0.3em]
0 \\[0.3em]
\end{bmatrix}
\end{equation*}


\subsection*{d)}
z is determined from the matrices that was obtained in \textbf{a)}
\begin{align*}
z=\sqrt{2}(U+H^{-1}Fx_k)=...=\begin{bmatrix}
\sqrt{2}(u_k+\frac{Qx_k}{Q+R}) \\ 
\sqrt{2}u_{k+1}
\end{bmatrix}
\end{align*}
The target function $f_0$ Then becomes:
\begin{align*}
f_0=z^THz=...=Ru_{k+1}^2+(Q+R)(u_k+\frac{Qx_k}{Q+R})^2
\end{align*}
The constraints
\begin{align*}
Gz\leq w+S\theta \Leftrightarrow 
\begin{bmatrix}
u_k+\frac{Qx_k}{Q+R} \\
u_{k+1} \\
-u_k - \frac{Qx_k}{Q+R}\\
-u_{k+1}
\end{bmatrix}
\leq
\begin{bmatrix}
1+\frac{Qx_k}{Q+R} \\
1 \\
1 - \frac{Qx_k}{Q+R}\\
1
\end{bmatrix}
\Leftrightarrow
\begin{cases}
u_k  \leq 1 \\
u_{k+1} \leq 1\\
u_k  \geq -1\\
u_{k+1} \geq -1
\end{cases}
\end{align*}
 Minimizing $f_0 \mbox{ w.r.t } u_k \mbox{ and } u_{k+1}$ by:
 
\begin{align*}
\nabla f_0=
\begin{bmatrix}
2(Q+R)(u_k+\frac{Qx_k}{Q+R})\\
2Ru_{k+1}
\end{bmatrix}
=\bar{0}
\Leftrightarrow 
\begin{cases}
u_k=-\frac{Q}{Q+R}x_k\\
u_{k+1}=0
\end{cases}
\end{align*}
To assure that this is a minimum the hessian is calculated
\begin{align*}
\nabla^2f_0=
\begin{bmatrix}
2(Q+R) & 0 \\
0 & 2R
\end{bmatrix}
\end{align*}
As the hessian is positive definite this is a minimum point.
By comparing to the constraints the following is obtained
\begin{align*}
\begin{cases}
-\frac{Q}{Q+R}x_k\geq u_k=1\\
-\frac{Q}{Q+R}x_k\leq u_k=-1
\end{cases}
\Leftrightarrow
\begin{cases}
x_k\leq -\frac{Q+R}{Q} \\
x_k\geq\frac{Q+R}{Q}
\end{cases}
\end{align*}
Then the optimal control is
\begin{align*}
u^*=
\begin{cases}
1 \quad x_k\leq -\frac{Q+R}{Q} \\
-\frac{Q}{Q+R}x_k \quad -\frac{Q+R}{Q} \leq x_k \leq \frac{Q+R}{Q} \\
-1 \quad x_k\geq\frac{Q+R}{Q}
\end{cases}
\end{align*}

\subsection*{e)}
By using the MPT toolbox in matlab with the following code:
\begin{verbatim*}
Z.A=1;
Z.B=1;
Z.C=1;
Z.D=0;
Z.umin=-1;
Z.umax=1;
Z.xmin=-4;
Z.xmax=4;
prob.norm=2;
prob.Q=2;
prob.R=1;
prob.N=2;
prob.Tconstraint=0;
prob.subopt_lev=0;
ctrlQ2R1=mpt_control(Z,prob);
prob.Q=1;
prob.R=2;
ctrlQ1R2=mpt_control(Z,prob);
subplot(2,1,2)
mpt_plotU(ctrlQ2R1)
title('Control signal for Q=2, R=1')
subplot(2,1,1)
mpt_plotU(ctrlQ1R2)
title('Control signal for Q=1, R=2')
\end{verbatim*}
The resulting plot for this simulation for the control signal can be seen in figure \ref{ctrl}.
The result is consistent with what was aquired in task \textbf{d)}
\begin{figure}[H]
\centering
\includegraphics[width=1.0\textwidth]{controlsignals}
\caption{The control signal u for different Q and R depending on x}
\label{ctrl}
\end{figure}
\subsection*{f)}
Suppose that Q and R are diagonal matrices. Then $f_0$ becomes the following expression.
\begin{equation*}
 f_0 = x_k^TQx_k + u_k^TRu_k
 = Qx_k^Tx_k + Ru_k^Tu_k
 \end{equation*}
 Because the scaling of the objective function is irrelevant for the optimization the expression becomes.
 \begin{equation*}
  f_0 = x_k^Tx_k + Q^{-1}Ru_k^Tu_k
  \end{equation*}
  Which shows that the ratio between Q and R is the only thing that matters. 






\end{document}
